package es.uji.viselab.visualservoing;

import java.awt.Button;
import java.awt.FlowLayout;
import java.awt.Label;
import java.awt.TextArea;
import java.awt.TextField;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.text.DecimalFormat;

import javax.swing.BoxLayout;
import javax.swing.JPanel;
import javax.swing.Timer;

import es.uji.viselab.LogLevel;
import es.uji.viselab.LogRecorder;
import es.uji.viselab.RobotListener;
import es.uji.viselab.ViSeLabException;
import es.uji.viselab.experiment.ExperimentParameters;
import es.uji.viselab.image.source.ImageSource;
import es.uji.viselab.math.PointXYZ;
import es.uji.viselab.math.RobotPose;
import es.uji.viselab.math.RobotToolLocation;
import es.uji.viselab.robot.RobotInterface;
import es.uji.viselab.robot.RobotException;
import es.uji.viselab.robot.RobotInterface;

public class PBVSEyeInHandRelative extends VisualServoingTask {

	protected static Label labelStatusRobot = new Label("    Robot not available    ");
	protected Label labelStatusLinkVS = new Label("Not receiving data");
	protected Label labelStatusVS = new Label("VS not active");
	protected Button buttonStartVS = new Button("Start VS");
	protected Label[] positionLabel = new Label[6];
	protected TextField[] positionTextValue = new TextField[6];
	protected PointXYZ[] latestPositions = new PointXYZ[10];
	protected PointXYZ latestPosition;
	protected int numPositions = 0;

	protected TextArea textAreaDataReceived = new TextArea(5, 40);

	protected static PBVSEyeInHandRelative instance;
	static RobotInterface robot;
	static boolean robotActive = false;
	protected boolean linkDataSourceActive = false;
	protected boolean vsActive = false;
	protected boolean interfaceActive = false;

	protected RobotToolLocation rtlInitial;
	protected Timer startTimer;
	protected Timer stopTimer;
	protected int countTimer;

	// This class follows the Singleton design pattern
	// There will be only one instance active
	private PBVSEyeInHandRelative() {
		// Exists only to defeat instantiation (Singleton Pattern)
	}

	public PBVSEyeInHandRelative(ExperimentParameters ep) {
		// TODO Auto-generated constructor stub
	}

	public static PBVSEyeInHandRelative getInstance() {
		if (instance == null) {
			instance = new PBVSEyeInHandRelative();
			initialize();
		}
		return instance;
	}

	public static boolean isInstantiated() {
		return (instance != null);
	}

	protected static void initialize() {
		// I need an instance of the robot to send to it the position
		RobotManager manager = RobotManager.getInstance();
		manager.addListener(instance);
		if (manager.isRobotConnected()) {
			try {
				robot = manager.getRobot();
				activateVSRobotLink(true);
			} catch (ViSeLabException e) {
				// If the robot not exists or is not connected, I should wait
				// and event of robotListenerConnected
			}
		}
	}

	public JPanel getVisualServoingPanel() {
		JPanel panel = new JPanel();
		panel.setLayout(new BoxLayout(panel, BoxLayout.PAGE_AXIS));

		JPanel line1 = new JPanel();
		line1.setLayout(new FlowLayout());
		((FlowLayout) line1.getLayout()).setAlignment(FlowLayout.CENTER);
		line1.add(labelStatusRobot);
		line1.add(labelStatusLinkVS);

		JPanel line2 = new JPanel();
		line2.setLayout(new FlowLayout());
		((FlowLayout) line2.getLayout()).setAlignment(FlowLayout.CENTER);
		line2.add(labelStatusVS);
		line2.add(buttonStartVS);

		buttonStartVS.addActionListener(this);

		JPanel line3 = new JPanel();
		line3.setLayout(new FlowLayout());
		((FlowLayout) line3.getLayout()).setAlignment(FlowLayout.CENTER);
		line3.add(new Label("Input"));

		JPanel line4 = new JPanel();
		line4.setLayout(new FlowLayout());
		((FlowLayout) line4.getLayout()).setAlignment(FlowLayout.CENTER);
		line4.add(textAreaDataReceived);

		JPanel line5 = new JPanel();
		line5.setLayout(new FlowLayout());
		((FlowLayout) line5.getLayout()).setAlignment(FlowLayout.CENTER);
		line5.add(new Label("Output"));

		JPanel line6 = new JPanel();
		line6.setLayout(new FlowLayout());

		positionLabel[0] = new Label("x");
		positionTextValue[0] = new TextField(5);
		positionLabel[1] = new Label("y");
		positionTextValue[1] = new TextField(5);
		positionLabel[2] = new Label("z");
		positionTextValue[2] = new TextField(5);

		line6.add(positionLabel[0]);
		line6.add(positionTextValue[0]);
		line6.add(positionLabel[1]);
		line6.add(positionTextValue[1]);
		line6.add(positionLabel[2]);
		line6.add(positionTextValue[2]);

		panel.add(line1);
		panel.add(line2);
		panel.add(line3);
		panel.add(line4);
		panel.add(line5);
		panel.add(line6);

		interfaceActive = true;

		return panel;
	}

	@Override
	public void robotListenerPositionChanged(RobotPose rp) {
		// TODO Auto-generated method stub

	}

	@Override
	public void robotListenerCreated(RobotInterface robot) {
		PBVSEyeInHandRelative.robot = robot;
	}

	@Override
	public void robotListenerConnected(RobotInterface robot) {
		RobotManager manager = RobotManager.getInstance();
		manager.addListener(this);
		if (manager.isRobotConnected()) {
			try {
				robot = manager.getRobot();
				activateVSRobotLink(true);
			} catch (ViSeLabException e) {
				// If the robot not exists or is not connected, I should wait
			}
		}
	}

	@Override
	public void robotListenerDisconnected() {
		activateVSRobotLink(false);
	}

	private static void activateVSRobotLink(boolean status) {
		robotActive = status;
		if (status)
			labelStatusRobot.setText(robot.getName());
		else
			labelStatusRobot.setText("Robot not available");
	}

	private void activateVSDataLink(boolean status) {
		linkDataSourceActive = status;
		if (status)
			labelStatusLinkVS.setText("Receiving data");
		else
			labelStatusLinkVS.setText("Not receiving data");
	}

	public void featurePoints(double x, double y, double z) {

		if (PBVSEyeInHandRelative.isInstantiated()) {
			if (!linkDataSourceActive)
				activateVSDataLink(true);
			if (interfaceActive && !vsActive) {
				textAreaDataReceived.setText("dx: " + format(x) + " dy: " + format(y) + " dz: "
						+ format(z) + "\n");
			}
			if (vsActive) {
				RobotToolLocation rtl = new RobotToolLocation(rtlInitial);

				PointXYZ p = new PointXYZ(z * 200, -x * 200, y * 200);
				if (p != null) {
					textAreaDataReceived.setText("dx: " + format(x) + " dy: " + format(y) + " dz: "
							+ format(z) + "\n" + "x: " + (int) (p.getX()) + " y: "
							+ (int) (p.getY()) + " z: " + (int) (p.getZ()));
					rtl.increaseLocation(p.getX(), p.getY(), p.getZ());

					if (interfaceActive) {
						positionTextValue[0].setText(format(rtl.getLocation().getX()));
						positionTextValue[1].setText(format(rtl.getLocation().getY()));
						positionTextValue[2].setText(format(rtl.getLocation().getZ()));
					}
					try {
						if (robot != null)
							LogRecorder.log(LogLevel.STATS, "PBVSEyeInHandRelative:robot.move(): " + rtl.getLocation().getX() + " "+ rtl.getLocation().getY() + " "+ rtl.getLocation().getZ() + " ");
							robot.move(rtl);
					} catch (RobotException e) {
						activateVSRobotLink(false);
						LogRecorder.log(LogLevel.ERROR, "Error when moving the robot",
								"PBVSEyeInHandRelative.featurePoints();");
					}
				}
			}
		}
	}

	private String format(double d) {
		DecimalFormat formatter = new DecimalFormat("0.000");
		String s = formatter.format(d);
		return s;
	}

	@Override
	public void actionPerformed(ActionEvent e) {

		if (e.getSource().equals(buttonStartVS)) {
			if (vsActive) {
				startTimer.stop();
				stopTimer.stop();
				buttonStartVS.setLabel("Start VS");
				labelStatusVS.setText("VS not active");
				vsActive = false;
			} else if (robotActive && linkDataSourceActive && interfaceActive) {
				if (!vsActive) {
					buttonStartVS.setLabel("Starting...");
					numPositions = 0;
					countTimer = 10;
					startTimer = new Timer(1000, this);
					startTimer.setInitialDelay(1000);
					startTimer.start();
				}
			} else {
				if (!linkDataSourceActive)
					labelStatusVS.setText("Need data !!");
				else if (!robotActive)
					labelStatusVS.setText("Need robot !!");
			}
		}

		if (e.getSource().equals(startTimer)) {
			countTimer--;
			labelStatusVS.setText("Active in " + String.valueOf(countTimer));
			if (countTimer == 0) {
				startTimer.stop();
				countTimer = 60*3;
				stopTimer = new Timer(1000, this);
				stopTimer.setDelay(1000);
				stopTimer.start();
				labelStatusVS.setText("Active");
				buttonStartVS.setLabel("Stop");
				// This will be the initial pose of the robot, all other
				// movements will be displacements from this
				try {
					rtlInitial = robot.getRobotToolLocation();
				} catch (RobotException ex) {
					activateVSRobotLink(false);
					LogRecorder.log(LogLevel.ERROR, "Error when getting the robot position.",
							"VisualServoing.actionPerformed();");
				}
				vsActive = true;
			}
		}
		if (e.getSource().equals(stopTimer)) {
			countTimer--;
			labelStatusVS.setText(" " + String.valueOf(countTimer));
			if (countTimer == 0) {
				stopTimer.stop();
				labelStatusVS.setText("Disabled");
				vsActive = false;
			}
		}
	}

	@Override
	public void featurePoints(Object o) {
		// TODO Auto-generated method stub
		
	}

	@Override
	public void cameraFeaturePointsChanged(ImageSource i) {
		// TODO Auto-generated method stub
		
	}

	@Override
	public double[] compute(ImageSource image) {
		// TODO Auto-generated method stub
		return null;
	}

	@Override
	public JPanel getPanel() {
		// TODO Auto-generated method stub
		return null;
	}
}
